home *** CD-ROM | disk | FTP | other *** search
/ Sprite 1984 - 1993 / Sprite 1984 - 1993.iso / src / machserver / 1.098 / mach / ds5000.md / machAsm.s < prev    next >
Text File  |  1991-08-09  |  57KB  |  2,296 lines
  1. /*
  2.  * machAsm.s --
  3.  *
  4.  *    Contains misc. assembler routines for the PMAX.
  5.  *
  6.  *    Copyright (C) 1989 Digital Equipment Corporation.
  7.  *    Permission to use, copy, modify, and distribute this software and
  8.  *    its documentation for any purpose and without fee is hereby granted,
  9.  *    provided that the above copyright notice appears in all copies.  
  10.  *    Digital Equipment Corporation makes no representations about the
  11.  *    suitability of this software for any purpose.  It is provided "as is"
  12.  *    without express or implied warranty.
  13.  *
  14.  * $Header: /sprite/src/kernel/mach/ds5000.md/RCS/machAsm.s,v 1.5 91/08/09 15:23:18 shirriff Exp $ SPRITE (DECWRL)
  15.  */
  16.  
  17. #include "machConst.h"
  18. #include "machAsmDefs.h"
  19. #include "vm3maxConst.h"
  20. #include <regdef.h>
  21.  
  22. /*----------------------------------------------------------------------------
  23.  *
  24.  * MachConfigCache --
  25.  *
  26.  *    Size the caches.
  27.  *
  28.  * Results:
  29.  *         None.
  30.  *
  31.  * Side effects:
  32.  *    The size of the data cache stored into machDataCacheSize and the
  33.  *    size of instruction cache stored into machInstCacheSize.
  34.  *
  35.  *----------------------------------------------------------------------------
  36.  */
  37. CONFIG_FRAME=    (4*4)+4+4        # 4 arg saves, ra, and a saved register
  38.     .globl    MachConfigCache
  39. MachConfigCache:
  40.     subu    sp,CONFIG_FRAME
  41.     sw    ra,CONFIG_FRAME-4(sp)        # Save return address.
  42.     sw    s0,CONFIG_FRAME-8(sp)        # Save s0 on stack.
  43.     mfc0    s0,MACH_COP_0_STATUS_REG    # Save status register.
  44.     mtc0    zero,MACH_COP_0_STATUS_REG    # Disable interrupts.
  45.     .set    noreorder
  46.     la    v0,1f
  47.     or    v0,VMMACH_PHYS_UNCACHED_START    # Run uncached.
  48.     j    v0
  49.     nop
  50.  
  51. 1:    jal    SizeCache            # Get the size of the d-cache.
  52.     nop
  53.     sw    v0,machDataCacheSize
  54.     nop                    # Make sure sw out of pipe
  55.     nop
  56.     nop
  57.     nop
  58.     li    v0,MACH_SR_SWAP_CACHES        # Swap caches
  59.     mtc0    v0,MACH_COP_0_STATUS_REG
  60.     nop                    # Insure caches stable
  61.     nop
  62.     nop
  63.     nop
  64.     jal    SizeCache            # Get the size of the i-cache.
  65.     nop
  66.     sw    v0,machInstCacheSize        
  67.     nop                    # Make sure sw out of pipe
  68.     nop
  69.     nop
  70.     nop
  71.     mtc0    zero, MACH_COP_0_STATUS_REG    # Swap back caches. 
  72.     nop
  73.     nop
  74.     nop
  75.     nop
  76.     la    t0,1f
  77.     j    t0                # Back to cached mode
  78.     nop
  79.  
  80. 1:    mtc0    s0,MACH_COP_0_STATUS_REG    # Restore status register.
  81.     nop
  82.     lw    s0,CONFIG_FRAME-8(sp)        # Restore old s0
  83.     lw    ra,CONFIG_FRAME-4(sp)        # Restore return addr
  84.     addu    sp,CONFIG_FRAME            # Restore sp.
  85.     j    ra
  86.     nop
  87.     .set    reorder
  88.  
  89. /*----------------------------------------------------------------------------
  90.  *
  91.  * SizeCache --
  92.  *
  93.  *    Get the size of the cache.
  94.  *
  95.  * Results:
  96.  *         The size of the cache.
  97.  *
  98.  * Side effects:
  99.  *    None.
  100.  *
  101.  *----------------------------------------------------------------------------
  102.  */
  103. SizeCache:
  104.     .set    noreorder
  105.     mfc0    t0,MACH_COP_0_STATUS_REG    # Save the current status reg.
  106.     nop                
  107.     or    v0,t0,MACH_SR_ISOL_CACHES    # Isolate the caches.
  108.     nop                    # Make sure no stores in pipe
  109.     mtc0    v0,MACH_COP_0_STATUS_REG
  110.     nop                    # Make sure isolated
  111.     nop
  112.     nop
  113.     /*
  114.      * Clear cache size boundaries.
  115.      */
  116.     li    v0, MACH_MIN_CACHE_SIZE
  117. 1:
  118.     sw    zero, VMMACH_PHYS_CACHED_START(v0)
  119.     sll    v0,1
  120.     ble    v0,+MACH_MAX_CACHE_SIZE,1b
  121.     nop
  122.     li    v0,-1
  123.     sw    v0, VMMACH_PHYS_CACHED_START(zero)    # Store marker in cache
  124.     li    v0, MACH_MIN_CACHE_SIZE
  125.  
  126. 2:    lw    v1, VMMACH_PHYS_CACHED_START(v0)    # Look for marker
  127.     nop            
  128.     bne    v1,zero,3f                # Found marker.
  129.     nop
  130.  
  131.     sll    v0,1            # cache size * 2
  132.     ble    v0,+MACH_MAX_CACHE_SIZE,2b        # keep looking
  133.     nop
  134.     move    v0,zero            # must be no cache
  135.     .set    reorder
  136.  
  137. 3:    mtc0    t0,MACH_COP_0_STATUS_REG
  138.     nop                # Make sure unisolated
  139.     nop
  140.     nop
  141.     nop
  142.     j    ra
  143.     nop
  144. .set reorder
  145.  
  146. /*----------------------------------------------------------------------------
  147.  *
  148.  * MachFlushCache --
  149.  *
  150.  *    Flush the caches.
  151.  *
  152.  * Results:
  153.  *         None.
  154.  *
  155.  * Side effects:
  156.  *    The contents of the cache is flushed.
  157.  *
  158.  *----------------------------------------------------------------------------
  159.  */
  160.     .globl    MachFlushCache
  161. MachFlushCache:
  162.     lw    t1,machInstCacheSize        # Must load before isolating
  163.     lw    t2,machDataCacheSize        # Must load before isolating
  164.     mfc0    t3,MACH_COP_0_STATUS_REG     # Save the status register.
  165.     mtc0    zero,MACH_COP_0_STATUS_REG    # Disable interrupts.
  166.     .set    noreorder
  167.     la    v0,1f
  168.     or    v0,VMMACH_PHYS_UNCACHED_START    # Run uncached.
  169.     j    v0            
  170.     nop
  171.  
  172.     /*
  173.      * flush text cache
  174.      */
  175. 1:    li    v0,MACH_SR_ISOL_CACHES|MACH_SR_SWAP_CACHES
  176.     mtc0    v0,MACH_COP_0_STATUS_REG    # Isolate and swap caches.
  177.     li    t0,VMMACH_PHYS_UNCACHED_START
  178.     subu    t0,t1
  179.     li    t1,VMMACH_PHYS_UNCACHED_START
  180.     la    v0,1f                # Run cached
  181.     j    v0
  182.     nop
  183.     .set    reorder
  184.  
  185. 1:    sb    zero,0(t0)
  186.     sb    zero,4(t0)
  187.     sb    zero,8(t0)
  188.     sb    zero,12(t0)
  189.     sb    zero,16(t0)
  190.     sb    zero,20(t0)
  191.     sb    zero,24(t0)
  192.     addu    t0,32
  193.     sb    zero,-4(t0)
  194.     bne    t0,t1,1b
  195.  
  196.     .set    noreorder
  197.     la    v0,1f
  198.     or    v0,VMMACH_PHYS_UNCACHED_START
  199.     j    v0                # Run uncached
  200.     nop
  201.  
  202.     /*
  203.      * flush data cache
  204.      */
  205. 1:    li    v0,MACH_SR_ISOL_CACHES|MACH_SR_SWAP_CACHES
  206.     mtc0    v0,MACH_COP_0_STATUS_REG    # Isolate and swap back caches
  207.     li    t0,VMMACH_PHYS_UNCACHED_START
  208.     subu    t0,t2
  209.     la    v0,1f
  210.     j    v0                # Back to cached mode
  211.     nop
  212.     .set    reorder
  213.  
  214. 1:    sb    zero,0(t0)
  215.     sb    zero,4(t0)
  216.     sb    zero,8(t0)
  217.     sb    zero,12(t0)
  218.     sb    zero,16(t0)
  219.     sb    zero,20(t0)
  220.     sb    zero,24(t0)
  221.     addu    t0,32
  222.     sb    zero,-4(t0)
  223.     bne    t0,t1,1b
  224.  
  225.     .set    noreorder
  226.     nop                    # Insure isolated stores 
  227.     nop                    #     out of pipe.
  228.     nop
  229.     mtc0    t3,MACH_COP_0_STATUS_REG    # Restore status reg.
  230.     nop                    # Insure cache unisolated.
  231.     nop
  232.     nop
  233.     nop
  234.     .set    reorder
  235.     j    ra
  236.  
  237. /*----------------------------------------------------------------------------
  238.  *
  239.  * MachCleanICache --
  240.  *
  241.  *    MachCleanICache(addr, len)
  242.  *
  243.  *    Flush i cache for range ofaddr to addr + len - 1.
  244.  *
  245.  * Results:
  246.  *         None.
  247.  *
  248.  * Side effects:
  249.  *    The contents of the cache is flushed.
  250.  *
  251.  *----------------------------------------------------------------------------
  252.  */
  253.     .globl MachCleanICache
  254. MachCleanICache:
  255.     lw    t1,machInstCacheSize
  256.     mfc0    t3,MACH_COP_0_STATUS_REG    # Save SR
  257.     mtc0    zero,MACH_COP_0_STATUS_REG    # Disable interrupts.
  258.  
  259.     .set    noreorder
  260.     la    v0,1f
  261.     or    v0,VMMACH_PHYS_UNCACHED_START    # Run uncached.
  262.     j    v0
  263.     nop
  264.  
  265. 1:    li    v0,MACH_SR_ISOL_CACHES|MACH_SR_SWAP_CACHES
  266.     mtc0    v0,MACH_COP_0_STATUS_REG
  267.     bltu    t1,a1,1f        # cache is smaller than region
  268.     nop
  269.     move    t1,a1
  270. 1:    addu    t1,a0            # ending address + 1
  271.     move    t0,a0
  272.     la    v0,1f            # run cached
  273.     j    v0
  274.     nop
  275.     .set    reorder
  276.  
  277. 1:    sb    zero,0(t0)
  278.     sb    zero,4(t0)
  279.     sb    zero,8(t0)
  280.     sb    zero,12(t0)
  281.     sb    zero,16(t0)
  282.     sb    zero,20(t0)
  283.     sb    zero,24(t0)
  284.     addu    t0,32
  285.     sb    zero,-4(t0)
  286.     bltu    t0,t1,1b
  287.  
  288.     .set    noreorder
  289.     la    v0,1f
  290.     or    v0,VMMACH_PHYS_UNCACHED_START
  291.     j    v0            # Run uncached
  292.     nop
  293.  
  294. 1:    nop                # insure isolated stores out of pipe
  295.     mtc0    zero,MACH_COP_0_STATUS_REG  # unisolate, unswap
  296.     nop                # keep pipeline clean
  297.     nop                # keep pipeline clean
  298.     nop                # keep pipeline clean
  299.     mtc0    t3,MACH_COP_0_STATUS_REG # enable interrupts
  300.     nop
  301.     j    ra            # return and run cached
  302.     nop
  303.     .set    reorder
  304.  
  305.     .globl MachFetchICache
  306. MachFetchICache:
  307.     mfc0    t3,MACH_COP_0_STATUS_REG    # Save SR
  308.     mtc0    zero,MACH_COP_0_STATUS_REG    # Disable interrupts.
  309.  
  310.     .set    noreorder
  311.     la    v0,1f
  312.     or    v0,VMMACH_PHYS_UNCACHED_START    # Run uncached.
  313.     j    v0
  314.     nop
  315.  
  316. 1:    li    v0,MACH_SR_ISOL_CACHES|MACH_SR_SWAP_CACHES
  317.     mtc0    v0,MACH_COP_0_STATUS_REG
  318.     la    v0,1f            # run cached
  319.     j    v0
  320.     nop
  321. 1:    ld    v0, 0(a0)
  322.     nop
  323.  
  324.     la    t0,1f
  325.     or    t0,VMMACH_PHYS_UNCACHED_START
  326.     j    t0            # Run uncached
  327.     nop
  328.  
  329. 1:    mtc0    zero, MACH_COP_0_STATUS_REG  # unisolate, unswap
  330.     nop                # keep pipeline clean
  331.     nop                # keep pipeline clean
  332.     nop                # keep pipeline clean
  333.     mtc0    t3,MACH_COP_0_STATUS_REG # enable interrupts
  334.     nop
  335.     j    ra            # return and run cached
  336.     nop
  337.     .set    reorder
  338.  
  339. /*----------------------------------------------------------------------------
  340.  *
  341.  * Mach_FlushCacheRange --
  342.  *
  343.  *    Mach_FlushCacheRange(addr, len)
  344.  *
  345.  *    Flush d cache for range ofaddr to addr + len - 1.
  346.  *
  347.  * Results:
  348.  *         None.
  349.  *
  350.  * Side effects:
  351.  *    The contents of the d cache is flushed.
  352.  *
  353.  *----------------------------------------------------------------------------
  354.  */
  355.     .globl Mach_FlushCacheRange
  356. Mach_FlushCacheRange:
  357.     lw    t1,machDataCacheSize
  358.     mfc0    t3,MACH_COP_0_STATUS_REG    # Save SR
  359.     mtc0    zero,MACH_COP_0_STATUS_REG    # Disable interrupts.
  360.  
  361.     .set    noreorder
  362.     la    v0,1f
  363.     or    v0,VMMACH_PHYS_UNCACHED_START    # Run uncached.
  364.     j    v0
  365.     nop
  366.  
  367. 1:    li    v0,MACH_SR_ISOL_CACHES    #isolate cache
  368.     mtc0    v0,MACH_COP_0_STATUS_REG
  369.     bltu    t1,a1,1f        # cache is smaller than region
  370.     nop
  371.     move    t1,a1
  372. 1:    addu    t1,a0            # ending address + 1
  373.     move    t0,a0
  374.     la    v0,1f            # run cached
  375.     j    v0
  376.     nop
  377.     .set    reorder
  378.  
  379. 1:    sb    zero,0(t0)
  380.     sb    zero,4(t0)
  381.     sb    zero,8(t0)
  382.     sb    zero,12(t0)
  383.     sb    zero,16(t0)
  384.     sb    zero,20(t0)
  385.     sb    zero,24(t0)
  386.     addu    t0,32
  387.     sb    zero,-4(t0)
  388.     bltu    t0,t1,1b
  389.  
  390.     .set    noreorder
  391.     la    v0,1f
  392.     or    v0,VMMACH_PHYS_UNCACHED_START
  393.     j    v0            # Run uncached
  394.     nop
  395.  
  396. 1:    nop                # insure isolated stores out of pipe
  397.     mtc0    zero,MACH_COP_0_STATUS_REG  # unisolate, unswap
  398.     nop                # keep pipeline clean
  399.     nop                # keep pipeline clean
  400.     nop                # keep pipeline clean
  401.     mtc0    t3,MACH_COP_0_STATUS_REG # enable interrupts
  402.     nop
  403.     j    ra            # return and run cached
  404.     nop
  405.     .set    reorder
  406.  
  407.  
  408.  
  409. /*----------------------------------------------------------------------------
  410.  *
  411.  * MachRunUserProc --
  412.  *
  413.  *    MachRunUserProc(pc, sp)
  414.  *        Address    pc;    * The program counter to execute at.
  415.  *        Address sp;    * The stack pointer to start with.
  416.  *
  417.  *    Start a process running in user mode.  We are called with interrupts
  418.  *    disabled.  
  419.  *
  420.  *
  421.  * Results:
  422.  *         None.
  423.  *
  424.  * Side effects:
  425.  *    The status register and stack pointer are modified.
  426.  *
  427.  *----------------------------------------------------------------------------
  428.  */
  429. LEAF(MachRunUserProc)
  430. .set noreorder
  431.     subu    sp, sp, STAND_FRAME_SIZE
  432.     sw        ra, STAND_RA_OFFSET(sp)
  433.     sw        a0, STAND_FRAME_SIZE(sp)
  434.     sw        a1, STAND_FRAME_SIZE + 4(sp)
  435.     .mask    0x80000000, -4
  436.     li        t0, (MACH_KERN_INT_MASK|MACH_SR_KU_PREV|MACH_SR_INT_ENA_PREV)
  437.     mtc0    t0, MACH_COP_0_STATUS_REG
  438.     lw        k0, machCurStatePtr
  439.     add        k1, a0, zero
  440. .set noat
  441.     RESTORE_REGS(k0, MACH_TRAP_REGS_OFFSET)
  442. .set at
  443.  
  444.     j        k1
  445.     rfe
  446. .set reorder
  447. END(MachRunUserProc)
  448.  
  449. /*----------------------------------------------------------------------------
  450.  *
  451.  * MachException --
  452.  *
  453.  *    Handle a general exception.
  454.  *
  455.  *
  456.  * Results:
  457.  *         None.
  458.  *
  459.  * Side effects:
  460.  *    None.
  461.  *
  462.  *----------------------------------------------------------------------------
  463.  */
  464.     .globl MachException
  465. MachException:
  466. .set noat
  467. /*
  468.  * Find out what mode we came from.
  469.  */
  470.     mfc0    k0, MACH_COP_0_STATUS_REG
  471.     and        k0, k0, MACH_SR_KU_PREV
  472.     bne        k0, zero, 1f
  473.     j        MachKernException
  474. 1:  j        MachUserException
  475. .set at
  476.     .globl MachEndException
  477. MachEndException:
  478.  
  479. /*----------------------------------------------------------------------------
  480.  *
  481.  * MachKernException --
  482.  *
  483.  *    Handle an exception from kernel mode.
  484.  *
  485.  * Results:
  486.  *         None.
  487.  *
  488.  * Side effects:
  489.  *    None.
  490.  *
  491.  *----------------------------------------------------------------------------
  492.  */
  493.     .globl MachKernException
  494. MachKernException:
  495. /*
  496.  * Determine the type of fault and jump to the appropriate routine.
  497.  */
  498. .set noreorder
  499. .set noat
  500.     mfc0    k0, MACH_COP_0_CAUSE_REG    # Get the cause register value.
  501.     la        k1, machKernExcTable        # Load base of the func table.
  502.     and        k0, k0, MACH_CR_EXC_CODE    # Mask out the cause bits. 
  503.     add        k0, k0, k1            # Get the address of the
  504.                         #    function entry.  Note that
  505.                         #    the cause is already 
  506.                         #    shifted left by 2 bits so
  507.                         #    we don't have to shift.
  508.     lw        k0, 0(k0)            # Get the function address
  509.     nop
  510.     j        k0                # Jump to the function.
  511.     nop
  512. .set at
  513. .set reorder
  514.  
  515.  
  516. /*----------------------------------------------------------------------------
  517.  *
  518.  * Mach_KernGenException --
  519.  *
  520.  *    Handle an exception from kernel mode.
  521.  *
  522.  * Results:
  523.  *         None.
  524.  *
  525.  * Side effects:
  526.  *    None.
  527.  *
  528.  *----------------------------------------------------------------------------
  529.  */
  530.  
  531. /*
  532.  * The kernel exception stack contains 28 saved general registers, the
  533.  * status register and the cause register and the multiply lo and high 
  534.  * registers.  In addition we need to set
  535.  * this up for linkage conventions.
  536.  */
  537. #define    KERN_EXC_FRAME_SIZE    (STAND_FRAME_SIZE + 8 + SAVED_REG_SIZE + 8)
  538. #define KERN_SR_OFFSET        (STAND_FRAME_SIZE)
  539. #define CAUSE_OFFSET        (STAND_FRAME_SIZE + 4)
  540. #define SAVED_REG_OFFSET    (STAND_FRAME_SIZE + 8)
  541. #define KERN_MULT_LO_OFFSET    (STAND_FRAME_SIZE + 8 + SAVED_REG_SIZE)
  542. #define KERN_MULT_HI_OFFSET    (STAND_FRAME_SIZE + 8 + SAVED_REG_SIZE + 4)
  543.  
  544. NON_LEAF(Mach_KernGenException,KERN_EXC_FRAME_SIZE,ra)
  545. .set noreorder
  546. .set noat
  547.     subu    sp, sp, KERN_EXC_FRAME_SIZE
  548. /*
  549.  * Save kernel registers onto the stack.
  550.  */
  551.     SAVE_KERNEL_REGS(SAVED_REG_OFFSET)
  552.     mflo    t0
  553.     sw        t0, KERN_MULT_LO_OFFSET(sp)
  554.     mfhi    t0
  555.     sw        t0, KERN_MULT_HI_OFFSET(sp)
  556. /*
  557.  * Save the rest of the state.
  558.  */
  559.     mfc0    k0, MACH_COP_0_EXC_PC
  560.     mfc0    k1, MACH_COP_0_STATUS_REG
  561.     sw        k0, STAND_RA_OFFSET(sp)
  562.     .mask    0x80000000, (STAND_RA_OFFSET - KERN_EXC_FRAME_SIZE)
  563.     sw        k1, KERN_SR_OFFSET(sp)
  564.     mfc0    k0, MACH_COP_0_CAUSE_REG
  565.     nop
  566.     sw        k0, CAUSE_OFFSET(sp)
  567.  
  568. /*
  569.  * Call the exception handler.
  570.  */
  571.     mfc0    a0, MACH_COP_0_STATUS_REG    # First arg is the status reg.
  572.     mfc0    a1, MACH_COP_0_CAUSE_REG    # Second arg is the cause reg.
  573.     mfc0    a2, MACH_COP_0_BAD_VADDR    # Third arg is the fault addr.
  574.     mfc0    a3, MACH_COP_0_EXC_PC        # Fourth arg is the pc.
  575.  
  576. /*
  577.  * Don't disable interrupts from the memory system, unless there is
  578.  * a memory system interrupt pending.
  579.  */
  580.     and        t0, a0, MACH_INT_MASK_3
  581.     bne        zero, t0, 10f
  582.     nop
  583.     mfc0    t0, MACH_COP_0_STATUS_REG
  584.     and        t0, t0, ~MACH_KERN_INT_MASK
  585.     or        t0, t0, MACH_INT_MASK_3 | MACH_SR_INT_ENA_CUR
  586.     mtc0    t0, MACH_COP_0_STATUS_REG
  587.     nop
  588.     nop
  589. 10:
  590.     jal        MachKernelExceptionHandler
  591.     nop
  592.     mtc0    zero, MACH_COP_0_STATUS_REG    # Disable interrupts
  593.     nop
  594. /*
  595.  * Check error code.
  596.  */
  597.     li        t0, MACH_OK
  598.     li        t1, MACH_USER_ERROR
  599.     beq        v0, t0, 9f
  600.     nop
  601.     beq        v0, t1, 8f
  602.     nop
  603.  
  604. /*
  605.  * We got a kernel error.  Save the special registers that we saved on 
  606.  * the stack into the debug state struct.
  607.  */
  608.     lw        k0, machDebugStatePtr
  609.     lw        k1, KERN_SR_OFFSET(sp)
  610.     nop
  611.     sw        k1, MACH_DEBUG_STATUS_REG_OFFSET(k0)
  612.     lw        k1, STAND_RA_OFFSET(sp)
  613.     nop
  614.     sw        k1, MACH_DEBUG_EXC_PC_OFFSET(k0)
  615.     lw        k1, CAUSE_OFFSET(sp)
  616.     nop
  617.     sw        k1, MACH_DEBUG_CAUSE_REG_OFFSET(k0)
  618.  
  619. /*
  620.  * Restore kernel registers and pop the stack.
  621.  */
  622.     lw        t0, KERN_MULT_LO_OFFSET(sp)
  623.     lw        t1, KERN_MULT_HI_OFFSET(sp)
  624.     mtlo    t0
  625.     mthi    t1
  626.     RESTORE_KERNEL_REGS(SAVED_REG_OFFSET)
  627.     addu    sp, sp, KERN_EXC_FRAME_SIZE
  628.  
  629. /*
  630.  * Save the general registers into the debug state struct.
  631.  */
  632.     SAVE_REGS(k0, MACH_DEBUG_REGS_OFFSET)
  633.     sw        gp, MACH_DEBUG_REGS_OFFSET +  (4 * GP)(k0)
  634.     sw        sp, MACH_DEBUG_REGS_OFFSET +  (4 * SP)(k0)
  635.     mflo    t0
  636.     sw        t0, MACH_DEBUG_MULT_LO_OFFSET(k0)
  637.     mfhi    t0
  638.     sw        t0, MACH_DEBUG_MULT_HI_OFFSET(k0)
  639.  
  640. /*
  641.  * Now save the rest of the special registers.
  642.  */
  643.     mfc0    t0, MACH_COP_0_TLB_INDEX
  644.     nop
  645.     sw        t0, MACH_DEBUG_TLB_INDEX_OFFSET(k0)
  646.     mfc0    t0, MACH_COP_0_TLB_RANDOM
  647.     nop
  648.     sw        t0, MACH_DEBUG_TLB_RANDOM_OFFSET(k0)
  649.     mfc0    t0, MACH_COP_0_TLB_LOW
  650.     nop
  651.     sw        t0, MACH_DEBUG_TLB_LOW_OFFSET(k0)
  652.     mfc0    t0, MACH_COP_0_TLB_CONTEXT
  653.     nop
  654.     sw        t0, MACH_DEBUG_TLB_CONTEXT_OFFSET(k0)
  655.     mfc0    t0, MACH_COP_0_BAD_VADDR
  656.     nop
  657.     sw        t0, MACH_DEBUG_BAD_VADDR_OFFSET(k0)
  658.     mfc0    t0, MACH_COP_0_TLB_HI
  659.     nop
  660.     sw        t0, MACH_DEBUG_TLB_HI_OFFSET(k0)
  661. /*
  662.  * Save the floating point state.
  663.  */
  664.  
  665. .set at
  666.     mfc0    t0, MACH_COP_0_STATUS_REG
  667.     nop
  668.     or        t0, t0, MACH_SR_COP_1_BIT
  669.     mtc0    t0, MACH_COP_0_STATUS_REG
  670.     nop
  671.     nop
  672.     cfc1    t1, MACH_FPC_CSR
  673.     nop
  674.     sw        t1, MACH_DEBUG_FPC_CSR_REG_OFFSET(k0)
  675.  
  676. #define SAVE_DEBUG_CP1_REG(reg) \
  677.     swc1    $f/**/reg, MACH_DEBUG_FP_REGS_OFFSET+reg*4(k0)
  678.  
  679.     SAVE_DEBUG_CP1_REG(0);  SAVE_DEBUG_CP1_REG(1);  SAVE_DEBUG_CP1_REG(2)
  680.     SAVE_DEBUG_CP1_REG(3);  SAVE_DEBUG_CP1_REG(4);  SAVE_DEBUG_CP1_REG(5)
  681.     SAVE_DEBUG_CP1_REG(6);  SAVE_DEBUG_CP1_REG(7);  SAVE_DEBUG_CP1_REG(8)
  682.     SAVE_DEBUG_CP1_REG(9);  SAVE_DEBUG_CP1_REG(10); SAVE_DEBUG_CP1_REG(11)
  683.     SAVE_DEBUG_CP1_REG(12); SAVE_DEBUG_CP1_REG(13); SAVE_DEBUG_CP1_REG(14)
  684.     SAVE_DEBUG_CP1_REG(15); SAVE_DEBUG_CP1_REG(16); SAVE_DEBUG_CP1_REG(17)
  685.     SAVE_DEBUG_CP1_REG(18); SAVE_DEBUG_CP1_REG(19); SAVE_DEBUG_CP1_REG(20)
  686.     SAVE_DEBUG_CP1_REG(21); SAVE_DEBUG_CP1_REG(22); SAVE_DEBUG_CP1_REG(23)
  687.     SAVE_DEBUG_CP1_REG(24); SAVE_DEBUG_CP1_REG(25); SAVE_DEBUG_CP1_REG(26)
  688.     SAVE_DEBUG_CP1_REG(27); SAVE_DEBUG_CP1_REG(28); SAVE_DEBUG_CP1_REG(29)
  689.     SAVE_DEBUG_CP1_REG(30); SAVE_DEBUG_CP1_REG(31)
  690. .set noat
  691.  
  692. /*
  693.  * Switch to the debuggers stack and call the debugger.  The debuggers
  694.  * stack starts at the base of the first kernel stack.
  695.  */
  696.     li        sp, MACH_STACK_BOTTOM - STAND_FRAME_SIZE
  697.     jal        Dbg_Main
  698.     nop
  699. /*
  700.  * The debugger returns the PC to continue at.
  701.  */
  702.     add        k1, v0, 0
  703.     lw        k0, machDebugStatePtr
  704.     nop
  705.  
  706.     lw        t0, MACH_TRAP_MULT_LO_OFFSET(k0)
  707.     lw        t1, MACH_TRAP_MULT_HI_OFFSET(k0)
  708.     mtlo    t0
  709.     mthi    t1
  710.     RESTORE_REGS(k0, MACH_DEBUG_REGS_OFFSET)
  711.     lw        k0, MACH_DEBUG_STATUS_REG_OFFSET(k0)
  712.     nop
  713.     mtc0    k0, MACH_COP_0_STATUS_REG    
  714.     nop
  715.     j        k1
  716.     rfe
  717.  
  718. 8:
  719. /*
  720.  * We got an error on a cross address space copy.  All we have to do is
  721.  * restore the stack pointer and the status register, set the return value
  722.  * register and return.
  723.  */
  724.     lw        t0, KERN_MULT_LO_OFFSET(sp)
  725.     lw        t1, KERN_MULT_HI_OFFSET(sp)
  726.     mtlo    t0
  727.     mthi    t1
  728.     RESTORE_KERNEL_REGS(SAVED_REG_OFFSET)
  729.     lui        v0, 0x2                # v0 <= SYS_ARG_NO_ACCESS
  730.     lw        k0, KERN_SR_OFFSET(sp)        # Get the saved sp.
  731.     addu    sp, sp, KERN_EXC_FRAME_SIZE    # Clear off the stack.
  732.     mtc0    k0, MACH_COP_0_STATUS_REG    # Restore the status register.
  733.     nop
  734.     j        ra                # Now return to the caller
  735.     rfe                        #   who caused the error.
  736.  
  737.  
  738. 9:
  739. /*
  740.  * Restore registers and return from the exception.
  741.  */
  742.     lw        t0, KERN_MULT_LO_OFFSET(sp)
  743.     lw        t1, KERN_MULT_HI_OFFSET(sp)
  744.     mtlo    t0
  745.     mthi    t1
  746.     RESTORE_KERNEL_REGS(SAVED_REG_OFFSET)
  747.  
  748.     lw        k0, KERN_SR_OFFSET(sp)
  749.     lw        k1, STAND_RA_OFFSET(sp)
  750.     addu    sp, sp, KERN_EXC_FRAME_SIZE
  751.     mtc0    k0, MACH_COP_0_STATUS_REG    # Restore the SR
  752.     nop
  753.     j        k1                # Now return from the
  754.     rfe                        #    exception.
  755. END(Mach_KernGenException)
  756.     .set at
  757.     .set reorder
  758.  
  759. /*----------------------------------------------------------------------------
  760.  *
  761.  * MachUserException --
  762.  *
  763.  *    Handle an exception from user mode.
  764.  *
  765.  * Results:
  766.  *         None.
  767.  *
  768.  * Side effects:
  769.  *    None.
  770.  *
  771.  *----------------------------------------------------------------------------
  772.  */
  773.     .globl MachUserException
  774. MachUserException:
  775. .set noreorder
  776. .set noat
  777.     mfc0    k0, MACH_COP_0_CAUSE_REG    # Get the cause register value.
  778.     la        k1, machUserExcTable        # Load base of the func table.
  779.     and        k0, k0, MACH_CR_EXC_CODE    # Mask out the cause bits. 
  780.     add        k0, k0, k1            # Get the address of the
  781.                         #    function entry.  Note that
  782.                         #    the cause is already 
  783.                         #    shifted left by 2 bits so
  784.                         #    we don't have to shift.
  785.     lw        k0, 0(k0)            # Get the function address
  786.     nop
  787.     j        k0                # Jump to the function.
  788.     nop
  789. .set at
  790. .set reorder
  791.  
  792. /*----------------------------------------------------------------------------
  793.  *
  794.  * Mach_UserGenException --
  795.  *
  796.  *    Handle an exception from user mode.
  797.  *
  798.  * Results:
  799.  *         None.
  800.  *
  801.  * Side effects:
  802.  *    None.
  803.  *
  804.  *----------------------------------------------------------------------------
  805.  */
  806. /*
  807.  * The user exception stack contains the status register and the exception
  808.  * PC.
  809.  */
  810. #define    USER_EXC_FRAME_SIZE    (4 + STAND_FRAME_SIZE)
  811. #define USER_SR_OFFSET        (STAND_RA_OFFSET + 4)
  812.  
  813. NON_LEAF(Mach_UserGenException,USER_EXC_FRAME_SIZE,ra)
  814. .set noreorder
  815. .set noat
  816. /*
  817.  * First of all switch over to the kernel gp.
  818.  */
  819.     add        k1, gp, zero
  820.     la        gp, _gp
  821.     lw        k0, machCurStatePtr
  822.     nop
  823. /*
  824.  * Save all registers.
  825.  */
  826.     sw        sp, MACH_TRAP_REGS_OFFSET + (SP * 4)(k0)
  827.     sw        k1, MACH_TRAP_REGS_OFFSET + (GP * 4)(k0)
  828.     SAVE_REGS(k0, MACH_TRAP_REGS_OFFSET)
  829.     mflo    t0
  830.     sw        t0, MACH_TRAP_MULT_LO_OFFSET(k0)
  831.     mfhi    t0
  832.     sw        t0, MACH_TRAP_MULT_HI_OFFSET(k0)
  833. .set at
  834.  
  835. /*
  836.  * Change to the kernel's stack.
  837.  */
  838.     lw        sp, MACH_KERN_STACK_END_OFFSET(k0)
  839. /*
  840.  * Set up the stack frame.
  841.  */
  842.     mfc0    a3, MACH_COP_0_EXC_PC        # The fourth arg is the PC
  843.     subu    sp, sp, USER_EXC_FRAME_SIZE
  844.     sw        a3, STAND_RA_OFFSET(sp)
  845.     sw        a3, MACH_USER_PC_OFFSET(k0)
  846.     .mask    0x80000000, (STAND_RA_OFFSET - USER_EXC_FRAME_SIZE)
  847.  
  848.     mfc0    a0, MACH_COP_0_STATUS_REG    # First arg is the status reg.
  849.     nop
  850.     and        k1, a0, ~MACH_SR_COP_1_BIT    # Turn off the FPU.
  851.     mtc0    k1, MACH_COP_0_STATUS_REG
  852.     sw        k1, USER_SR_OFFSET(sp)
  853.  
  854. /*
  855.  * Call the handler.
  856.  */
  857.     mfc0    a1, MACH_COP_0_CAUSE_REG    # Second arg is the cause reg.
  858.     mfc0    a2, MACH_COP_0_BAD_VADDR    # Third arg is the fault addr
  859.     jal        MachUserExceptionHandler
  860.     nop
  861.  
  862. /*
  863.  * Restore user registers and return.  Interrupts are already disabled
  864.  * when MachUserExceptionHandler returns.
  865.  */
  866.     lw        k0, USER_SR_OFFSET(sp)
  867.     beq        v0, zero, 1f            # See if we are supposed to
  868.     nop                        #   to turn on the FPU.
  869.     or        k0, k0, MACH_SR_COP_1_BIT
  870. 1:
  871.     mtc0    k0, MACH_COP_0_STATUS_REG
  872.     lw        k0, machCurStatePtr
  873.     nop
  874. .set noat
  875.     lw        t0, MACH_TRAP_MULT_LO_OFFSET(k0)
  876.     lw        t1, MACH_TRAP_MULT_HI_OFFSET(k0)
  877.     mtlo    t0
  878.     mthi    t1
  879.     RESTORE_REGS(k0, MACH_TRAP_REGS_OFFSET)
  880.     lw        k1, MACH_USER_PC_OFFSET(k0)
  881.     nop
  882.     j        k1
  883.     rfe
  884. END(Mach_UserGenException)
  885. .set at
  886. .set reorder
  887.  
  888.  
  889. /*----------------------------------------------------------------------------
  890.  *
  891.  * Mach_EnableIntr --
  892.  *
  893.  *    Enable interrupts.
  894.  *
  895.  * Results:
  896.  *         None.
  897.  *
  898.  * Side effects:
  899.  *    Interrupts enabled.
  900.  *
  901.  *----------------------------------------------------------------------------
  902.  */
  903. LEAF(Mach_EnableIntr)
  904. .set noreorder
  905.     mfc0    t0, MACH_COP_0_STATUS_REG
  906.     nop
  907.     or        t0, t0, MACH_KERN_INT_MASK | MACH_SR_INT_ENA_CUR
  908.     mtc0    t0, MACH_COP_0_STATUS_REG
  909.     nop
  910.     j        ra
  911.     nop
  912. .set reorder
  913. END(Mach_EnableIntr)
  914.  
  915. /*----------------------------------------------------------------------------
  916.  *
  917.  * Mach_DisableIntr --
  918.  *
  919.  *    Disable Interrupts.
  920.  *
  921.  * Results:
  922.  *         None.
  923.  *
  924.  * Side effects:
  925.  *    Interrupts disabled.
  926.  *
  927.  *----------------------------------------------------------------------------
  928.  */
  929. LEAF(Mach_DisableIntr)
  930. .set noreorder
  931.     mfc0    t0, MACH_COP_0_STATUS_REG
  932.     nop
  933.     and        t0, t0, ~(MACH_SR_INT_ENA_CUR | MACH_KERN_INT_MASK)
  934.     or        t0, t0, MACH_INT_MASK_3 | MACH_SR_INT_ENA_CUR
  935.     mtc0    t0, MACH_COP_0_STATUS_REG
  936.     nop
  937.     j        ra
  938.     nop
  939. .set reorder
  940. END(Mach_DisableIntr)
  941.  
  942. /*----------------------------------------------------------------------------
  943.  *
  944.  * Mach_ContextSwitch --
  945.  *
  946.  *    Mach_ContextSwitch(fromProcPtr, toProcPtr)
  947.  *
  948.  *    Perform a context switch.
  949.  *
  950.  * Results:
  951.  *         None.
  952.  *
  953.  * Side effects:
  954.  *    The current process's state is saved into its machine specific
  955.  *    process table entry and new state is loaded for the switched to
  956.  *    process.  
  957.  *
  958.  *----------------------------------------------------------------------------
  959.  */
  960. .set noreorder
  961.  
  962. NON_LEAF(Mach_ContextSwitch,STAND_FRAME_SIZE + 8,ra)
  963.     subu    sp, sp, STAND_FRAME_SIZE
  964.     sw        ra, STAND_RA_OFFSET(sp)
  965.     sw        a0, STAND_FRAME_SIZE(sp)
  966.     sw        a1, STAND_FRAME_SIZE + 4(sp)
  967.     .mask    0x80000000, -4
  968. /*
  969.  * Set up this processes context.
  970.  */
  971.     add        a0, a1, zero
  972.     jal        VmMach_SetupContext
  973.     nop
  974. /*
  975.  * Restore saved register values.
  976.  */
  977.     lw        ra, STAND_RA_OFFSET(sp)
  978.     lw        a0, STAND_FRAME_SIZE(sp)
  979.     lw        a1, STAND_FRAME_SIZE + 4(sp)
  980. /*
  981.  * Push the magic number and the status register onto the stack.
  982.  */
  983.     subu    sp, sp, 8
  984.     li        t0, MAGIC
  985.     sw        t0, 0(sp)
  986.     mfc0    t0, MACH_COP_0_STATUS_REG # Save the status reg.
  987.     nop
  988.     sw        t0, 4(sp)
  989.  
  990. /*
  991.  * Save the state of the current process.  We only have to save the saved
  992.  * register registers (s0 through s8) and the stack pointer.
  993.  */
  994.     lw        t0, machCurStatePtr
  995.     nop
  996.     add        t0, t0, MACH_SWITCH_REGS_OFFSET
  997.     sw        a0, A0 * 4(t0)
  998.     sw        s0, S0 * 4(t0)
  999.     sw        s1, S1 * 4(t0)
  1000.     sw        s2, S2 * 4(t0)
  1001.     sw        s3, S3 * 4(t0)
  1002.     sw        s4, S4 * 4(t0)
  1003.     sw        s5, S5 * 4(t0)
  1004.     sw        s6, S6 * 4(t0)
  1005.     sw        s7, S7 * 4(t0)
  1006.     sw        s8, S8 * 4(t0)
  1007.     sw        ra, RA * 4(t0)
  1008.     sw        sp, SP * 4(t0)
  1009.     .globl Mach_SwitchPoint
  1010. Mach_SwitchPoint:
  1011.  
  1012. /*
  1013.  * Restore the registers for the new process.
  1014.  */
  1015.     lw        t0, machStatePtrOffset
  1016.     nop
  1017.     add        t0, a1, t0
  1018.     lw        t0, 0(t0)
  1019.     nop
  1020.     sw        t0, machCurStatePtr
  1021.     add        t1, t0, MACH_SWITCH_REGS_OFFSET
  1022.     lw        a0, A0 * 4(t1)
  1023.     lw        s0, S0 * 4(t1)
  1024.     lw        s1, S1 * 4(t1)
  1025.     lw        s2, S2 * 4(t1)
  1026.     lw        s3, S3 * 4(t1)
  1027.     lw        s4, S4 * 4(t1)
  1028.     lw        s5, S5 * 4(t1)
  1029.     lw        s6, S6 * 4(t1)
  1030.     lw        s7, S7 * 4(t1)
  1031.     lw        s8, S8 * 4(t1)
  1032.     lw        ra, RA * 4(t1)
  1033.     lw        sp, SP * 4(t1)
  1034. /*
  1035.  * Set up the maximum stack addr for the debugger.
  1036.  */
  1037.     lw        t1, MACH_KERN_STACK_END_OFFSET(t0)
  1038.     nop
  1039.     sw        t1, dbgMaxStackAddr
  1040.  
  1041. /*
  1042.  * Wire down the current process's stack in the TLB.  This
  1043.  * code depends implicitly upon MACH_KERN_STACK_PAGES.
  1044.  */
  1045.     mfc0    t1, VMMACH_TLB_HI
  1046.  
  1047. /*
  1048.  * Map the first entry.
  1049.  */
  1050.     lw        t2, MACH_TLB_HIGH_ENTRY_OFFSET(t0)
  1051.     lw        t3, MACH_TLB_LOW_ENTRY_1_OFFSET(t0)
  1052.     li        t4, MACH_STACK_TLB_INDEX_1
  1053.     mtc0    t2, VMMACH_TLB_HI
  1054.     mtc0    t3, VMMACH_TLB_LOW
  1055.     mtc0    t4, VMMACH_TLB_INDEX
  1056.     nop
  1057.     tlbwi
  1058.  
  1059. /*
  1060.  * Map the second entry.
  1061.  */
  1062.     addu    t2, t2, 1 << VMMACH_TLB_VIRT_PAGE_SHIFT
  1063.     lw        t3, MACH_TLB_LOW_ENTRY_2_OFFSET(t0)
  1064.     li        t4, MACH_STACK_TLB_INDEX_2
  1065.     mtc0    t2, VMMACH_TLB_HI
  1066.     mtc0    t3, VMMACH_TLB_LOW
  1067.     mtc0    t4, VMMACH_TLB_INDEX
  1068.     nop
  1069.     tlbwi
  1070.  
  1071. /*
  1072.  * Map the third entry.
  1073.  */
  1074.     addu    t2, t2, 1 << VMMACH_TLB_VIRT_PAGE_SHIFT
  1075.     lw        t3, MACH_TLB_LOW_ENTRY_3_OFFSET(t0)
  1076.     li        t4, MACH_STACK_TLB_INDEX_3
  1077.     mtc0    t2, VMMACH_TLB_HI
  1078.     mtc0    t3, VMMACH_TLB_LOW
  1079.     mtc0    t4, VMMACH_TLB_INDEX
  1080.     nop
  1081.     tlbwi
  1082.  
  1083.     mtc0    t1, VMMACH_TLB_HI
  1084.  
  1085. /*
  1086.  * Verify the magic number on the stack.
  1087.  */
  1088.     lw        t0, 0(sp)
  1089.     li        t1, MAGIC
  1090.     beq        t0, t1, 1f
  1091.     nop
  1092.     break    0
  1093.  
  1094. /*
  1095.  * Restore the status register and pop the stack.
  1096.  */
  1097. 1:
  1098.     lw        t0, 4(sp)
  1099.     nop
  1100.     mtc0    t0, MACH_COP_0_STATUS_REG
  1101.     add        sp, sp, STAND_FRAME_SIZE + 8
  1102. /*
  1103.  * Return 
  1104.  */
  1105.     j        ra
  1106.     nop
  1107.  
  1108. END(Mach_ContextSwitch)
  1109. .set reorder
  1110.  
  1111. /*----------------------------------------------------------------------------
  1112.  *
  1113.  * Mach_GetPC --
  1114.  *
  1115.  *    Return the caller's PC.
  1116.  *
  1117.  * Results:
  1118.  *         The PC of the caller.
  1119.  *
  1120.  * Side effects:
  1121.  *    None.
  1122.  *
  1123.  *----------------------------------------------------------------------------
  1124.  */
  1125.     .globl Mach_GetPC
  1126. Mach_GetPC:
  1127.     add        v0, ra, zero
  1128.     j        ra
  1129.  
  1130. /*----------------------------------------------------------------------------
  1131.  *
  1132.  * Mach_GetCallerPC --
  1133.  *
  1134.  *    Return the caller's caller's PC.
  1135.  *
  1136.  * Results:
  1137.  *         I have no idea how to do this, so return 0 for now.
  1138.  *
  1139.  * Side effects:
  1140.  *    None.
  1141.  *
  1142.  *----------------------------------------------------------------------------
  1143.  */
  1144.     .globl Mach_GetCallerPC
  1145. Mach_GetCallerPC:
  1146.     add        v0, zero, zero
  1147.     j        ra
  1148.  
  1149. /*----------------------------------------------------------------------------
  1150.  *
  1151.  * Mach_TestAndSet --
  1152.  *
  1153.  *    Mach_TestAndSet(intPtr)
  1154.  *
  1155.  *    Return the caller's caller's PC.
  1156.  *
  1157.  * Results:
  1158.  *         0 for now.
  1159.  *
  1160.  * Side effects:
  1161.  *    None.
  1162.  *
  1163.  *----------------------------------------------------------------------------
  1164.  */
  1165. LEAF(Mach_TestAndSet)
  1166.     mfc0    t0, MACH_COP_0_STATUS_REG
  1167.     mtc0    zero, MACH_COP_0_STATUS_REG    # Disable interrupts
  1168.     lw        v0, 0(a0)            # Read out old value
  1169.     li        t1, 1
  1170.     sw        t1, 0(a0)            # Set value.
  1171.     mtc0    t0, MACH_COP_0_STATUS_REG    # Restore interrupts.
  1172.     j        ra
  1173. END(Mach_TestAndSet)
  1174.  
  1175. /*----------------------------------------------------------------------------
  1176.  *
  1177.  * Mach_EmptyWriteBuffer --
  1178.  *
  1179.  *    Mach_EmptyWriteBuffer()
  1180.  *
  1181.  *    Return when the write buffer is empty.
  1182.  *
  1183.  * Results:
  1184.  *         None.
  1185.  *
  1186.  * Side effects:
  1187.  *    None.
  1188.  *
  1189.  *----------------------------------------------------------------------------
  1190.  */
  1191. LEAF(Mach_EmptyWriteBuffer)
  1192.     nop
  1193.     nop
  1194.     nop
  1195.     nop
  1196. 1:    bc0f    1b
  1197.     j    ra
  1198. END(Mach_EmptyWriteBuffer)
  1199.  
  1200. #define SAVE_CP1_REG(reg) \
  1201.     swc1    $f/**/reg, MACH_FP_REGS_OFFSET+reg*4(a0)
  1202.  
  1203. #define REST_CP1_REG(reg) \
  1204.     lwc1    $f/**/reg, MACH_FP_REGS_OFFSET+reg*4(a1)
  1205.  
  1206. /*----------------------------------------------------------------------------
  1207.  *
  1208.  * MachSwitchFPState --
  1209.  *
  1210.  *    MachSwitchFPState(fromFPStatePtr, toFPStatePtr)
  1211.  *
  1212.  *    Save the current state into fromFPStatePtrs state and restore it
  1213.  *    from toFPStatePtr.
  1214.  *
  1215.  * Results:
  1216.  *         None.
  1217.  *
  1218.  * Side effects:
  1219.  *    None.
  1220.  *
  1221.  *----------------------------------------------------------------------------
  1222.  */
  1223. LEAF(MachSwitchFPState)
  1224.     subu    sp, sp, STAND_FRAME_SIZE
  1225.     sw    ra, STAND_RA_OFFSET(sp)
  1226.     .mask    0x80000000, (STAND_RA_OFFSET - STAND_FRAME_SIZE)
  1227.  
  1228.     mfc0    t1, MACH_COP_0_STATUS_REG    # Disable interrupts and
  1229.     li    t0, MACH_SR_COP_1_BIT        #    enable the coprocessor
  1230.     mtc0    t0, MACH_COP_0_STATUS_REG
  1231.  
  1232.     add    t0, a0, 1    # If fromFPStatePtr is NIL then it will equal
  1233.     beq    t0, zero, 1f    #    zero if we add one to it.
  1234.  
  1235. .set noreorder
  1236. /*
  1237.  * First read out the status register to make sure that all FP operations
  1238.  * have completed.
  1239.  */
  1240.     cfc1    t0, MACH_FPC_CSR
  1241.     nop
  1242.     sw    t0, MACH_FP_SR_OFFSET(a0)
  1243. /* 
  1244.  * Save the floating point registers.
  1245.  */
  1246.     SAVE_CP1_REG(0); SAVE_CP1_REG(1); SAVE_CP1_REG(2); SAVE_CP1_REG(3)
  1247.     SAVE_CP1_REG(4); SAVE_CP1_REG(5); SAVE_CP1_REG(6); SAVE_CP1_REG(7)
  1248.     SAVE_CP1_REG(8); SAVE_CP1_REG(9); SAVE_CP1_REG(10); SAVE_CP1_REG(11)
  1249.     SAVE_CP1_REG(12); SAVE_CP1_REG(13); SAVE_CP1_REG(14); SAVE_CP1_REG(15)
  1250.     SAVE_CP1_REG(16); SAVE_CP1_REG(17); SAVE_CP1_REG(18); SAVE_CP1_REG(19)
  1251.     SAVE_CP1_REG(20); SAVE_CP1_REG(21); SAVE_CP1_REG(22); SAVE_CP1_REG(23)
  1252.     SAVE_CP1_REG(24); SAVE_CP1_REG(25); SAVE_CP1_REG(26); SAVE_CP1_REG(27)
  1253.     SAVE_CP1_REG(28); SAVE_CP1_REG(29); SAVE_CP1_REG(30); SAVE_CP1_REG(31)
  1254.  
  1255. 1:    
  1256. /*
  1257.  * Restore the floating point registers.
  1258.  */
  1259.     REST_CP1_REG(0); REST_CP1_REG(1); REST_CP1_REG(2); REST_CP1_REG(3)
  1260.     REST_CP1_REG(4); REST_CP1_REG(5); REST_CP1_REG(6); REST_CP1_REG(7)
  1261.     REST_CP1_REG(8); REST_CP1_REG(9); REST_CP1_REG(10); REST_CP1_REG(11)
  1262.     REST_CP1_REG(12); REST_CP1_REG(13); REST_CP1_REG(14); REST_CP1_REG(15)
  1263.     REST_CP1_REG(16); REST_CP1_REG(17); REST_CP1_REG(18); REST_CP1_REG(19)
  1264.     REST_CP1_REG(20); REST_CP1_REG(21); REST_CP1_REG(22); REST_CP1_REG(23)
  1265.     REST_CP1_REG(24); REST_CP1_REG(25); REST_CP1_REG(26); REST_CP1_REG(27)
  1266.     REST_CP1_REG(28); REST_CP1_REG(29); REST_CP1_REG(30); REST_CP1_REG(31)
  1267.  
  1268.     lw    t0, MACH_FP_SR_OFFSET(a1)
  1269.     nop
  1270.     and    t0, t0, ~MACH_FPC_EXCEPTION_BITS
  1271.     ctc1    t0, MACH_FPC_CSR
  1272.     nop
  1273.     mtc0    t1, MACH_COP_0_STATUS_REG    # Restore the status register.
  1274.  
  1275.     addu    sp, sp, STAND_FRAME_SIZE
  1276.  
  1277.     j    ra
  1278.     nop
  1279.  
  1280. .set reorder
  1281. END(MachSwitchFPState)
  1282.  
  1283. /*----------------------------------------------------------------------------
  1284.  *
  1285.  * MachGetCurFPState --
  1286.  *
  1287.  *    MachGetCurFPState(statePtr)
  1288.  *
  1289.  *    Save the current state into *statePtr.
  1290.  *
  1291.  * Results:
  1292.  *         None.
  1293.  *
  1294.  * Side effects:
  1295.  *    None.
  1296.  *
  1297.  *----------------------------------------------------------------------------
  1298.  */
  1299. LEAF(MachGetCurFPState)
  1300.     subu    sp, sp, STAND_FRAME_SIZE
  1301.     sw    ra, STAND_RA_OFFSET(sp)
  1302.     .mask    0x80000000, (STAND_RA_OFFSET - STAND_FRAME_SIZE)
  1303.  
  1304.     mfc0    t1, MACH_COP_0_STATUS_REG    # Disable interrupts and
  1305.     li    t0, MACH_SR_COP_1_BIT        #    enable the coprocessor
  1306.     mtc0    t0, MACH_COP_0_STATUS_REG
  1307.  
  1308. .set noreorder
  1309.     /*
  1310.      * First read out the status register to make sure that all FP
  1311.      * operations have completed.
  1312.      */
  1313.     cfc1    t0, MACH_FPC_CSR
  1314.     nop
  1315.     sw    t0, MACH_FP_SR_OFFSET(a0)
  1316.     /* 
  1317.      * Save the floating point registers.
  1318.      */
  1319.     SAVE_CP1_REG(0); SAVE_CP1_REG(1); SAVE_CP1_REG(2); SAVE_CP1_REG(3)
  1320.     SAVE_CP1_REG(4); SAVE_CP1_REG(5); SAVE_CP1_REG(6); SAVE_CP1_REG(7)
  1321.     SAVE_CP1_REG(8); SAVE_CP1_REG(9); SAVE_CP1_REG(10); SAVE_CP1_REG(11)
  1322.     SAVE_CP1_REG(12); SAVE_CP1_REG(13); SAVE_CP1_REG(14); SAVE_CP1_REG(15)
  1323.     SAVE_CP1_REG(16); SAVE_CP1_REG(17); SAVE_CP1_REG(18); SAVE_CP1_REG(19)
  1324.     SAVE_CP1_REG(20); SAVE_CP1_REG(21); SAVE_CP1_REG(22); SAVE_CP1_REG(23)
  1325.     SAVE_CP1_REG(24); SAVE_CP1_REG(25); SAVE_CP1_REG(26); SAVE_CP1_REG(27)
  1326.     SAVE_CP1_REG(28); SAVE_CP1_REG(29); SAVE_CP1_REG(30); SAVE_CP1_REG(31)
  1327.  
  1328.     mtc0    t1, MACH_COP_0_STATUS_REG    # Restore the status register.
  1329.  
  1330.     addu    sp, sp, STAND_FRAME_SIZE
  1331.  
  1332.     j    ra
  1333.     nop
  1334.  
  1335. .set reorder
  1336. END(MachGetCurFPState)
  1337.  
  1338. /*----------------------------------------------------------------------------
  1339.  *
  1340.  * MachFPInterrupt --
  1341.  *
  1342.  *    Handle a floating point interrupt.
  1343.  *
  1344.  * Results:
  1345.  *         MACH_OK
  1346.  *
  1347.  * Side effects:
  1348.  *    None.
  1349.  *
  1350.  *----------------------------------------------------------------------------
  1351.  */
  1352. NON_LEAF(MachFPInterrupt,STAND_FRAME_SIZE,ra)
  1353. /*
  1354.  *    unsigned statusReg;    (in a0)
  1355.  *    unsigned causeReg;    (in a1)
  1356.  *    Address EXC_pc;        (in a2)
  1357.  */
  1358.     subu    sp, sp, STAND_FRAME_SIZE
  1359.     sw    ra, STAND_RA_OFFSET(sp)
  1360.         sw    a2, STAND_FRAME_SIZE + 8(sp)
  1361.  
  1362.     and    t1, a0, MACH_SR_KU_PREV
  1363.     bne    t1, zero, 1f
  1364.     /*
  1365.      * We got an FPU interrupt in kernel mode.
  1366.      * At this point we should do something clever like
  1367.      * simulating the instruction that failed.  Instead
  1368.      * we just clear the FPU status register and let
  1369.      * the job die.
  1370.      */
  1371.     PRINTF("FPU interrupt in Kernel mode\012")
  1372.     /*
  1373.      * Turn on the floating point coprocessor.
  1374.      */
  1375.     mfc0    t0, MACH_COP_0_STATUS_REG
  1376.     or    t1, t0, MACH_SR_COP_1_BIT
  1377.     mtc0    t1, MACH_COP_0_STATUS_REG
  1378.     /*
  1379.      * Some nops are needed here or else the coprocessor
  1380.      * won't be enabled by the time we try to clear the
  1381.      * status register and the kernel will panic with
  1382.      * "coprocessor unusable".
  1383.      */
  1384.     nop
  1385.     nop
  1386.     nop
  1387.     nop
  1388.     /*
  1389.      * Clear the status register.
  1390.      */
  1391.     ctc1    zero, MACH_FPC_CSR
  1392.     j    FPReturn
  1393. 1:
  1394.     /*
  1395.      * Turn on the floating point coprocessor.
  1396.      */
  1397.     mfc0    t0, MACH_COP_0_STATUS_REG
  1398.     or    t1, t0, MACH_SR_COP_1_BIT
  1399.     mtc0    t1, MACH_COP_0_STATUS_REG
  1400.     /* 
  1401.      * Check for a stray interrupt.
  1402.      */
  1403.     lw    t1, machFPCurStatePtr
  1404.     lw    t2, machCurStatePtr
  1405.     beq    t1, t2, 1f
  1406.     /*
  1407.      * We got an interrupt and no one was using the coprocessor.  Clear
  1408.      * the interrupt and complain.
  1409.      */
  1410.     PRINTF("Stray FPU interrupt\012")
  1411.     ctc1    zero, MACH_FPC_CSR
  1412.     j    FPReturn
  1413. 1:
  1414.     /*
  1415.      * Fetch the instruction.
  1416.      */
  1417.     add    v0, a1, 0
  1418.     bltz    v0, 3f                # Check the branch delay bit.
  1419.     /*
  1420.      * This is not in the branch delay slot so calculate the resulting
  1421.      * PC (epc + 4) into v0 and continue to softfp().
  1422.      */
  1423.     lw    a1, 0(a2)
  1424.     addu    v0, a2, 4
  1425.     lw    t0, machCurStatePtr
  1426.     sw    v0, MACH_USER_PC_OFFSET(t0)
  1427.     b    4f
  1428. 3:
  1429.     /*
  1430.      * This is in the branch delay slot so the branch will have to
  1431.      * be emulated to get the resulting PC.
  1432.      */
  1433.     lw    a0, machCurStatePtr
  1434.     add    a0, a0, MACH_TRAP_REGS_OFFSET
  1435.     add    a1, a2, zero
  1436.     cfc1    a2, MACH_FPC_CSR
  1437.     add    a3, zero, zero
  1438.     jal    MachEmulateBranch    # MachEmulateBranch(regsPtr,instPC,csr,
  1439.                     #            FALSE)
  1440.     lw    t0, machCurStatePtr
  1441.     sw    v0, MACH_USER_PC_OFFSET(t0)
  1442.     /*
  1443.      * Now load the floating-point instruction in the branch delay slot
  1444.      * to be emulated by softfp().
  1445.      */
  1446.     lw    a2, STAND_FRAME_SIZE + 8(sp)    #EXC pc
  1447.     lw    a1, 4(a2)
  1448. 4:
  1449.     /*
  1450.      * Check to see if the instruction to be emulated is a floating-point
  1451.      * instruction.
  1452.      */
  1453.     srl    a3, a1, MACH_OPCODE_SHIFT
  1454.     beq    a3, MACH_OPCODE_C1, 5f
  1455.     /*
  1456.      * Send a floating point exception signal to the current process.
  1457.      */
  1458.     li    a0, MACH_SIGFPE
  1459.     jal    Mach_SendSignal
  1460.     j    FPReturn
  1461.  
  1462. 5:
  1463.     /*
  1464.      * Finally we can call softfp() where a1 has the instruction to
  1465.      * emulate.
  1466.      */
  1467.     jal    softfp
  1468.  
  1469. FPReturn:
  1470.     /*
  1471.      * Turn off the floating point coprocessor.
  1472.      */
  1473.     mfc0    t0, MACH_COP_0_STATUS_REG
  1474.     and    t0, t0, ~MACH_SR_COP_1_BIT
  1475.     mtc0    t0, MACH_COP_0_STATUS_REG
  1476.     /*
  1477.      * Return to our caller.
  1478.      */
  1479.     lw    ra, STAND_RA_OFFSET(sp)
  1480.     addu    sp, sp, STAND_FRAME_SIZE
  1481.     j    ra
  1482.     li    v0, MACH_OK
  1483. END(MachFPInterrupt)
  1484.  
  1485. /*----------------------------------------------------------------------------
  1486.  *
  1487.  * MachSysCall --
  1488.  *
  1489.  *    MachSysCall --
  1490.  *
  1491.  *    Handle a system call.
  1492.  *
  1493.  * Results:
  1494.  *         None.
  1495.  *
  1496.  * Side effects:
  1497.  *    None.
  1498.  *
  1499.  *----------------------------------------------------------------------------
  1500.  */
  1501. .set noreorder
  1502.     .globl MachSysCall
  1503.     .ent MachSysCall, 0
  1504. MachSysCall:
  1505. /*
  1506.  * Check the magic number.
  1507.  */
  1508.     li        k0, MACH_SYSCALL_MAGIC
  1509.     bne        t1, k0, UNIXSyscall
  1510.     nop
  1511. 1:
  1512.     add        t7, gp, zero            # Save the user's gp in t7
  1513.     la        gp, _gp                # Switch to the kernel's gp
  1514. /*
  1515.  * See if this system call is valid.
  1516.  */
  1517.     lw        t2, machMaxSysCall        # t2 <= Maximum sys call value.
  1518.     nop
  1519.     add        t2, t2, 1            
  1520.     sltu    t2, t0, t2            # Is t0 < t2 ?    
  1521.     bne        t2, zero, 1f            # If so then continue on.
  1522.     nop
  1523. /*
  1524.  * System call number is too big.  Return SYS_INVALID_SYSTEM_CALL to
  1525.  * the user.
  1526.  */
  1527.     mfc0    t3, MACH_COP_0_EXC_PC
  1528.     add        gp, t7, zero
  1529.     li        v0, 0x20002
  1530.     add        t3, t3, 4
  1531.     j        t3
  1532.     rfe
  1533. /* 
  1534.  * Now we know that we have a good system call number so go ahead and
  1535.  * save state and switch to the kernel's stack.
  1536.  */
  1537. 1:
  1538.     lw        t1, machCurStatePtr
  1539.     add        t2, sp, zero
  1540.     mfc0    t3, MACH_COP_0_EXC_PC
  1541.     sw        sp, MACH_TRAP_REGS_OFFSET + (SP * 4)(t1)
  1542.     sw        t7, MACH_TRAP_REGS_OFFSET + (GP * 4)(t1)
  1543.     sw        s0, MACH_TRAP_REGS_OFFSET + (S0 * 4)(t1)
  1544.     sw        s1, MACH_TRAP_REGS_OFFSET + (S1 * 4)(t1)
  1545.     sw        s2, MACH_TRAP_REGS_OFFSET + (S2 * 4)(t1)
  1546.     sw        s3, MACH_TRAP_REGS_OFFSET + (S3 * 4)(t1)
  1547.     sw        s4, MACH_TRAP_REGS_OFFSET + (S4 * 4)(t1)
  1548.     sw        s5, MACH_TRAP_REGS_OFFSET + (S5 * 4)(t1)
  1549.     sw        s6, MACH_TRAP_REGS_OFFSET + (S6 * 4)(t1)
  1550.     sw        s7, MACH_TRAP_REGS_OFFSET + (S7 * 4)(t1)
  1551.     sw        s8, MACH_TRAP_REGS_OFFSET + (S8 * 4)(t1)
  1552.     sw        ra, MACH_TRAP_REGS_OFFSET + (RA * 4)(t1)
  1553.     sw        t0, MACH_TRAP_REGS_OFFSET + (T0 * 4)(t1)
  1554.     sw        a0, MACH_TRAP_REGS_OFFSET + (A0 * 4)(t1)
  1555.     sw        a1, MACH_TRAP_REGS_OFFSET + (A1 * 4)(t1)
  1556.     sw        a2, MACH_TRAP_REGS_OFFSET + (A2 * 4)(t1)
  1557.     sw        a3, MACH_TRAP_REGS_OFFSET + (A3 * 4)(t1)
  1558.     sw        v0, MACH_TRAP_REGS_OFFSET + (V0 * 4)(t1)
  1559.     sw        v1, MACH_TRAP_REGS_OFFSET + (V1 * 4)(t1)
  1560.     sw        t3, MACH_USER_PC_OFFSET(t1)
  1561. /*
  1562.  * Change to the kernel's stack, enable interrupts and turn off the
  1563.  * floating point coprocessor.
  1564.  */
  1565.     mfc0    s8, MACH_COP_0_STATUS_REG
  1566.     lw        sp, MACH_KERN_STACK_END_OFFSET(t1)
  1567.     and        s8, s8, ~MACH_SR_COP_1_BIT
  1568.     or        t3, s8, MACH_SR_INT_ENA_CUR
  1569.     mtc0    t3, MACH_COP_0_STATUS_REG
  1570. /*
  1571.  * Now fetch the args.  The user's stack pointer is in t2.
  1572.  */
  1573.     sll        t0, t0, 2
  1574.     la        t3, machArgDispatch
  1575.     add        t3, t0, t3
  1576.     lw        t3, 0(t3)
  1577.     nop
  1578.     jal        t3
  1579.     add        v0, zero, zero
  1580.     bne        v0, zero, sysCallReturn
  1581.     add        s0, t1, zero            # Save pointer to current state
  1582.                         #    in s0
  1583. /* 
  1584.  * We got the args now call the routine.
  1585.  */
  1586.     lw        s2, proc_RunningProcesses    # s2 <= pointer to running
  1587.                         #       processes array.
  1588.     lw        s1, machKcallTableOffset    # s1 <= Offset of kcall table
  1589.                         #       in proc table entry.
  1590.     lw        s2, 0(s2)            # s2 <= pointer to currently
  1591.     nop                        #       running process
  1592.     add        s3, s2, s1            # s3 <= pointer to kcall table
  1593.                         #       pointer for currently
  1594.                         #       running    process
  1595.     add        s1, s3, 4            # Special handling flag follows
  1596.                         # kcallTable field.  Save a 
  1597.                         # pointer to it in s1.
  1598.     lw        s3, 0(s3)            # s3 <= pointer to kcall table
  1599.     nop
  1600.     add        s3, s3, t0            # s3 <= pointer to pointer to
  1601.                         #       function to call.
  1602.     lw        s3, 0(s3)            # s3 <= pointer to function.
  1603.     nop
  1604.     jal        s3                # Call the function
  1605.     nop
  1606. /*
  1607.  * Return to the user.  We have following saved information:
  1608.  *
  1609.  *    s0:    machCurStatePtr
  1610.  *    s1:    procPtr->specialHandling
  1611.  *    s2:    procPtr
  1612.  *    s8:    status register
  1613.  */
  1614. sysCallReturn:
  1615.     mtc0    s8, MACH_COP_0_STATUS_REG    # Disable interrupts.
  1616.     nop
  1617.     lw        t0, 0(s1)            # Get special handling flag.
  1618.     nop
  1619.     beq        t0, zero, checkFP        # See if special handling 
  1620.     nop                        #    required
  1621. /*
  1622.  * Need some special handling.
  1623.  */
  1624.     lw        t1, MACH_USER_PC_OFFSET(s0)    # Fetch return PC.
  1625.     or        t0, s8, MACH_SR_INT_ENA_CUR    # Prepare to enable interrupts.
  1626.     add        t1, t1, 4            # Increment return PC by 4 to
  1627.                         #    get past the syscall inst.
  1628.     sw        t1, MACH_USER_PC_OFFSET(s0)    # Write back the return PC.
  1629.  
  1630.     mtc0    t0, MACH_COP_0_STATUS_REG    # Enable interrupts.
  1631.     sw        v0, MACH_TRAP_REGS_OFFSET + (V0 * 4)(s0)
  1632.  
  1633.     add        a0, s2, zero
  1634.     jal        MachUserReturn            # Call MachUserReturn(procPtr)
  1635.     nop
  1636. /*
  1637.  * Restore A0, A1 and A2 because these will get changed if a signal handler
  1638.  * is to be called.
  1639.  */
  1640.     lw        k0, MACH_USER_PC_OFFSET(s0)
  1641.     lw        a0, MACH_TRAP_REGS_OFFSET + (A0 * 4)(s0)
  1642.     lw        a1, MACH_TRAP_REGS_OFFSET + (A1 * 4)(s0)
  1643.     lw        a2, MACH_TRAP_REGS_OFFSET + (A2 * 4)(s0)
  1644. /*
  1645.  * V1 and A3 are restored for UNIX binary compatibility.
  1646.  */
  1647.     lw        v1, MACH_TRAP_REGS_OFFSET + (V1 * 4)(s0)
  1648.     lw        a3, MACH_TRAP_REGS_OFFSET + (A3 * 4)(s0)
  1649.  
  1650.     beq        v0, zero, sysCallRestore
  1651.     lw        v0, MACH_TRAP_REGS_OFFSET + (V0 * 4)(s0)
  1652.     or        s8, s8, MACH_SR_COP_1_BIT
  1653.     j        sysCallRestore
  1654.     nop
  1655.  
  1656. checkFP:
  1657.     lw        k0, MACH_USER_PC_OFFSET(s0)
  1658.     lw        t0, machFPCurStatePtr
  1659.     add        k0, k0, 4
  1660.     bne        t0, s0, sysCallRestore
  1661.     nop
  1662.     or        s8, s8, MACH_SR_COP_1_BIT
  1663.  
  1664. /*
  1665.  * Restore the registers.
  1666.  */
  1667.  
  1668. sysCallRestore:
  1669.     mtc0    s8, MACH_COP_0_STATUS_REG
  1670.     lw        sp, MACH_TRAP_REGS_OFFSET + (SP * 4)(s0)
  1671.     lw        gp, MACH_TRAP_REGS_OFFSET + (GP * 4)(s0)
  1672.     lw        s1, MACH_TRAP_REGS_OFFSET + (S1 * 4)(s0)
  1673.     lw        s2, MACH_TRAP_REGS_OFFSET + (S2 * 4)(s0)
  1674.     lw        s3, MACH_TRAP_REGS_OFFSET + (S3 * 4)(s0)
  1675.     lw        s4, MACH_TRAP_REGS_OFFSET + (S4 * 4)(s0)
  1676.     lw        s5, MACH_TRAP_REGS_OFFSET + (S5 * 4)(s0)
  1677.     lw        s6, MACH_TRAP_REGS_OFFSET + (S6 * 4)(s0)
  1678.     lw        s7, MACH_TRAP_REGS_OFFSET + (S7 * 4)(s0)
  1679.     lw        s8, MACH_TRAP_REGS_OFFSET + (S8 * 4)(s0)
  1680.     lw        ra, MACH_TRAP_REGS_OFFSET + (RA * 4)(s0)
  1681.     lw        s0, MACH_TRAP_REGS_OFFSET + (S0 * 4)(s0)
  1682. /*
  1683.  * Return.
  1684.  */
  1685.     j        k0
  1686.     rfe
  1687.  
  1688. .end MachSysCall
  1689. .set reorder
  1690.  
  1691. /*----------------------------------------------------------------------------
  1692.  *
  1693.  * UNIXSyscall --
  1694.  *
  1695.  *    Handle a UNIX system call.
  1696.  *
  1697.  * Results:
  1698.  *         None.
  1699.  *
  1700.  * Side effects:
  1701.  *    None.
  1702.  *
  1703.  *----------------------------------------------------------------------------
  1704.  */
  1705. .set noreorder
  1706.     .globl UNIXSyscall
  1707.     .ent UNIXSyscall, 0
  1708. UNIXSyscall:
  1709.  
  1710. /*
  1711.  * If we are using the new unix compatiblity stuff, then jump
  1712.  * to the new unix syscall handler.
  1713.  */
  1714.     lw          k0, machNewUnixCompat
  1715.     nop
  1716.     bne         k0, zero, newUNIXSyscall
  1717.     nop
  1718. /*
  1719.  * If we are tracing system calls are we have a signal or long jump return
  1720.  * do it the slow way.  Signal and long jump returns are done the slow way
  1721.  * because they have to do a full restore.
  1722.  */
  1723.     lw        k0, machUNIXSyscallTrace
  1724.     beq        v0, MACH_UNIX_LONG_JUMP_RETURN, Mach_UserGenException
  1725.     nop
  1726.     beq        v0, MACH_UNIX_SIG_RETURN, Mach_UserGenException
  1727.     nop
  1728.     bne        k0, zero, Mach_UserGenException
  1729.     nop
  1730.  
  1731.     add        t7, gp, zero            # Save the user's gp in t7
  1732.     la        gp, _gp                # Switch to the kernel's gp
  1733. /*
  1734.  * See if this system call is valid.
  1735.  */
  1736.     lw        t0, machNumUNIXSyscalls        # t0 <= Maximum sys call value.
  1737.     nop
  1738.     add        t0, t0, 1            
  1739.     sltu    t0, v0, t0            # Is v0 < t0 ?
  1740.     bne        t0, zero, 1f            # If so then continue on.
  1741.     nop
  1742. /*
  1743.  * System call number is too big.  Return EINVAL to
  1744.  * the user.
  1745.  */
  1746.     mfc0    t0, MACH_COP_0_EXC_PC
  1747.     add        gp, t7, zero
  1748.     li        v0, 22
  1749.     li        a3, 1
  1750.     add        t0, t0, 4
  1751.     j        t0
  1752.     rfe
  1753. /* 
  1754.  * Now we know that we have a good system call number so go ahead and
  1755.  * save state and switch to the kernel's stack.  Note that we save 
  1756.  * a0 - a2 and v1 because UNIX system call stubs assume that these
  1757.  * won't get modified unless a value is returned in v1.
  1758.  */
  1759. 1:
  1760.     sll        t0, v0, 2    # t0 <= v0 * 4
  1761.     sll        t3, v0, 3    # t3 <= v0 * 8
  1762.     add        t0, t0, t3    # t0 <= v0 * 12
  1763.     la        t3, machUNIXSysCallTable
  1764.     add        t0, t0, t3
  1765.     lw        t3, 4(t0)    # t3 <= number of arguments.
  1766.     nop
  1767.     bltz    t3, 42f
  1768.     nop
  1769.  
  1770.  
  1771.     lw        t1, machCurStatePtr
  1772.     add        t2, sp, zero
  1773.     mfc0    t3, MACH_COP_0_EXC_PC
  1774.     sw        sp, MACH_TRAP_REGS_OFFSET + (SP * 4)(t1)
  1775.     sw        t7, MACH_TRAP_REGS_OFFSET + (GP * 4)(t1)
  1776.     sw        s0, MACH_TRAP_REGS_OFFSET + (S0 * 4)(t1)
  1777.     sw        s1, MACH_TRAP_REGS_OFFSET + (S1 * 4)(t1)
  1778.     sw        s2, MACH_TRAP_REGS_OFFSET + (S2 * 4)(t1)
  1779.     sw        s3, MACH_TRAP_REGS_OFFSET + (S3 * 4)(t1)
  1780.     sw        s4, MACH_TRAP_REGS_OFFSET + (S4 * 4)(t1)
  1781.     sw        s5, MACH_TRAP_REGS_OFFSET + (S5 * 4)(t1)
  1782.     sw        s6, MACH_TRAP_REGS_OFFSET + (S6 * 4)(t1)
  1783.     sw        s7, MACH_TRAP_REGS_OFFSET + (S7 * 4)(t1)
  1784.     sw        s8, MACH_TRAP_REGS_OFFSET + (S8 * 4)(t1)
  1785.     sw        ra, MACH_TRAP_REGS_OFFSET + (RA * 4)(t1)
  1786.     sw        a0, MACH_TRAP_REGS_OFFSET + (A0 * 4)(t1)
  1787.     sw        a1, MACH_TRAP_REGS_OFFSET + (A1 * 4)(t1)
  1788.     sw        a2, MACH_TRAP_REGS_OFFSET + (A2 * 4)(t1)
  1789.     sw        v1, MACH_TRAP_REGS_OFFSET + (V1 * 4)(t1)
  1790.     sw        t3, MACH_USER_PC_OFFSET(t1)
  1791. /*
  1792.  * Change to the kernel's stack, enable interrupts and turn off the
  1793.  * floating point coprocessor.
  1794.  */
  1795.     mfc0    s8, MACH_COP_0_STATUS_REG
  1796.     lw        sp, MACH_KERN_STACK_END_OFFSET(t1)
  1797.     and        s8, s8, ~MACH_SR_COP_1_BIT
  1798.     or        t3, s8, MACH_SR_INT_ENA_CUR
  1799.     mtc0    t3, MACH_COP_0_STATUS_REG
  1800. /*
  1801.  * Now fetch the args.  The user's stack pointer is in t2 and the 
  1802.  * current state pointer in t1.
  1803.  */
  1804.     sll        t0, v0, 2    # t0 <= v0 * 4
  1805.     sll        t3, v0, 3    # t3 <= v0 * 8
  1806.     add        t0, t0, t3    # t0 <= v0 * 12
  1807.     la        t3, machUNIXSysCallTable
  1808.     add        t0, t0, t3
  1809.     lw        t3, 4(t0)    # t3 <= number of arguments.
  1810.     add        s3, v0, zero    # Save syscall type in s3.
  1811.     sll        t3, t3, 2
  1812.     la        t4, machArgDispatchTable
  1813.     add        t3, t3, t4
  1814.     lw        t3, 0(t3)    # t3 <= pointer to arg fetch routine.
  1815.     nop
  1816.     jal        t3
  1817.     add        v0, zero, zero
  1818.     bne        v0, zero, unixSyscallReturn
  1819.     add        s0, t1, zero            # Save pointer to current state
  1820.                         #    in s0
  1821.  
  1822. /* 
  1823.  * We got the args now call the routine.
  1824.  */
  1825.     lw        t3, 8(t0)    # t3 <= routine to call.
  1826.     sw        zero, MACH_TRAP_UNIX_RET_VAL_OFFSET(s0)
  1827.     jal        t3        # Call the routine.
  1828.     nop
  1829.  
  1830. /*
  1831.  * Return to the user.  We have the following saved information:
  1832.  *    s0:    machCurStatePtr
  1833.  *    s3:    syscall type.
  1834.  *    s8:    status register.
  1835.  */
  1836. unixSyscallReturn:
  1837.     lw        s2, proc_RunningProcesses    # s2 <= pointer to running
  1838.                         #       processes array.
  1839.     lw        s1, machKcallTableOffset    # s1 <= Offset of kcall table
  1840.                         #       in proc table entry.
  1841.     lw        s2, 0(s2)            # s2 <= pointer to currently
  1842.                         #       running process
  1843.     add        s1, s1, 4            # Special handling flag follows
  1844.                         # kcallTable field. 
  1845.     add        s1, s2, s1            # s1 <= pointer to special
  1846.                         #       handling flag.
  1847. /*
  1848.  * We now have the following saved information:
  1849.  *
  1850.  *    s0:    machCurStatePtr
  1851.  *    s1:    procPtr->specialHandling
  1852.  *    s2:    procPtr
  1853.  *    s3:    syscall type.
  1854.  *    s8:    status register
  1855.  */
  1856. /*
  1857.  * Set up the registers correctly:
  1858.  *
  1859.  *    1) Restore a0, a1, a2 and v1
  1860.  *    2) If status == 0 then regs[a3] <= 0 and v0 <= return value.
  1861.  *       Else regs[A3] <= 1 and v0 <= Compat_MapCode(status).
  1862.  */
  1863. 1:
  1864.     bne        v0, zero, 1f
  1865.     nop
  1866.     lw        v0, MACH_TRAP_UNIX_RET_VAL_OFFSET(s0)
  1867.     lw        v1, MACH_TRAP_REGS_OFFSET + (V1 * 4)(s0)
  1868.     lw        a0, MACH_TRAP_REGS_OFFSET + (A0 * 4)(s0)
  1869.     lw        a1, MACH_TRAP_REGS_OFFSET + (A1 * 4)(s0)
  1870.     lw        a2, MACH_TRAP_REGS_OFFSET + (A2 * 4)(s0)
  1871.     add        a3, zero, zero
  1872.     sw        v0, MACH_TRAP_REGS_OFFSET + (V0 * 4)(s0)
  1873.     sw        a3, MACH_TRAP_REGS_OFFSET + (A3 * 4)(s0)
  1874.     j        sysCallReturn
  1875.     nop
  1876. 1:
  1877.     jal        Compat_MapCode
  1878.     add        a0, v0, zero
  1879.     sw        v0, MACH_TRAP_UNIX_RET_VAL_OFFSET(s0)
  1880.     lw        v1, MACH_TRAP_REGS_OFFSET + (V1 * 4)(s0)
  1881.     lw        a0, MACH_TRAP_REGS_OFFSET + (A0 * 4)(s0)
  1882.     lw        a1, MACH_TRAP_REGS_OFFSET + (A1 * 4)(s0)
  1883.     lw        a2, MACH_TRAP_REGS_OFFSET + (A2 * 4)(s0)
  1884.     li        a3, 1
  1885.     sw        a3, MACH_TRAP_REGS_OFFSET + (A3 * 4)(s0)
  1886.     j        sysCallReturn
  1887.     nop
  1888.  
  1889.  
  1890. /*----------------------------------------------------------------------------
  1891.  *
  1892.  * newUNIXSyscall --
  1893.  *
  1894.  *      Handle a new (compatibility) UNIX system call.
  1895.  *      This code is part of UNIXSyscall, not a separate subroutine.
  1896.  *
  1897.  * Results:
  1898.  *      None.
  1899.  *
  1900.  * Side effects:
  1901.  *      None.
  1902.  *
  1903.  *----------------------------------------------------------------------------
  1904.  */
  1905.  
  1906. newUNIXSyscall:
  1907.     sw          v0, sysCallNum
  1908.     add        t7, gp, zero            # Save the user's gp in t7
  1909.     la        gp, _gp                # Switch to the kernel's gp
  1910. /*
  1911.  * If we are tracing system calls or we have a signal or long jump return
  1912.  * do it the slow way.  Signal and long jump returns are done the slow way
  1913.  * because they have to do a full restore.
  1914.  */
  1915.     lw        k0, machUNIXSyscallTrace
  1916.     beq        v0, MACH_UNIX_LONG_JUMP_RETURN, Mach_UserGenException
  1917.     nop
  1918.     beq        v0, MACH_UNIX_SIG_RETURN, Mach_UserGenException
  1919.     nop
  1920.     bne        k0, zero, Mach_UserGenException
  1921. /*
  1922.  * See if this system call is valid.
  1923.  */
  1924. 42:
  1925.     sltu    t0, v0, MACH_MAX_UNIX_SYSCALL   # t0 <= Maximum sys call value.
  1926.     bne        t0, zero, 1f            # If so then continue on.
  1927.     nop
  1928. /*
  1929.  * System call number is too big.  Return EINVAL to
  1930.  * the user.
  1931.  */
  1932.     mfc0    t0, MACH_COP_0_EXC_PC
  1933.     add        gp, t7, zero
  1934.     li        v0, 22
  1935.     li        a3, 1
  1936.     add        t0, t0, 4
  1937.     j        t0
  1938.     rfe
  1939. /* 
  1940.  * Now we know that we have a good system call number so go ahead and
  1941.  * save state and switch to the kernel's stack.  Note that we save 
  1942.  * a0 - a2 and v1 because UNIX system call stubs assume that these
  1943.  * won't get modified unless a value is returned in v1.
  1944.  */
  1945. 1:
  1946.     lw        t1, machCurStatePtr
  1947.     add        t2, sp, zero
  1948.     mfc0    t3, MACH_COP_0_EXC_PC
  1949.     sw        v0, MACH_TRAP_UNIX_RET_VAL_OFFSET+4(t1)
  1950.     sw        a3, MACH_TRAP_UNIX_RET_VAL_OFFSET+8(t1)
  1951.     sw        sp, MACH_TRAP_REGS_OFFSET + (SP * 4)(t1)
  1952.     sw        t7, MACH_TRAP_REGS_OFFSET + (GP * 4)(t1)
  1953.     sw        s0, MACH_TRAP_REGS_OFFSET + (S0 * 4)(t1)
  1954.     sw        s1, MACH_TRAP_REGS_OFFSET + (S1 * 4)(t1)
  1955.     sw        s2, MACH_TRAP_REGS_OFFSET + (S2 * 4)(t1)
  1956.     sw        s3, MACH_TRAP_REGS_OFFSET + (S3 * 4)(t1)
  1957.     sw        s4, MACH_TRAP_REGS_OFFSET + (S4 * 4)(t1)
  1958.     sw        s5, MACH_TRAP_REGS_OFFSET + (S5 * 4)(t1)
  1959.     sw        s6, MACH_TRAP_REGS_OFFSET + (S6 * 4)(t1)
  1960.     sw        s7, MACH_TRAP_REGS_OFFSET + (S7 * 4)(t1)
  1961.     sw        s8, MACH_TRAP_REGS_OFFSET + (S8 * 4)(t1)
  1962.     sw        ra, MACH_TRAP_REGS_OFFSET + (RA * 4)(t1)
  1963.     sw        a0, MACH_TRAP_REGS_OFFSET + (A0 * 4)(t1)
  1964.     sw        a1, MACH_TRAP_REGS_OFFSET + (A1 * 4)(t1)
  1965.     sw        a2, MACH_TRAP_REGS_OFFSET + (A2 * 4)(t1)
  1966.     sw        v1, MACH_TRAP_REGS_OFFSET + (V1 * 4)(t1)
  1967.     sw        t3, MACH_USER_PC_OFFSET(t1)
  1968. /*
  1969.  * Change to the kernel's stack, enable interrupts and turn off the
  1970.  * floating point coprocessor.
  1971.  */
  1972.     mfc0    s8, MACH_COP_0_STATUS_REG
  1973.     lw        sp, MACH_KERN_STACK_END_OFFSET(t1)
  1974.     and        s8, s8, ~MACH_SR_COP_1_BIT
  1975.     or        t3, s8, MACH_SR_INT_ENA_CUR
  1976.     mtc0    t3, MACH_COP_0_STATUS_REG
  1977. /*
  1978.  * Now fetch the args.  The user's stack pointer is in t2 and the 
  1979.  * current state pointer in t1.
  1980.  */
  1981.     sll        t0, v0, 3    # t0 <= v0 * 8
  1982.     la        t3, sysUnixSysCallTable
  1983.     add        t0, t0, t3
  1984.     lw        t3, 4(t0)    # t3 <= number of arguments.
  1985.     add        s3, v0, zero    # Save syscall type in s3.
  1986.     sll        t3, t3, 2
  1987.     la        t4, machArgDispatchTable
  1988.     add        t3, t3, t4
  1989.     lw        t3, 0(t3)    # t3 <= pointer to arg fetch routine.
  1990.     nop
  1991.     jal        t3
  1992.     add        v0, zero, zero
  1993.     bne        v0, zero, unixNewSyscallReturn
  1994.     add        s0, t1, zero            # Save pointer to current state
  1995.                         #    in s0
  1996.  
  1997. /* 
  1998.  * We got the args now call the routine.
  1999.  */
  2000.     lw        t3, 0(t0)    # t3 <= routine to call.
  2001.     nop
  2002.     jal        t3        # Call the routine.
  2003.     nop
  2004.  
  2005. /*
  2006.  * Return to the user.  We have the following saved information:
  2007.  *    s0:    machCurStatePtr
  2008.  *    s3:    syscall type.
  2009.  *    s8:    status register.
  2010.  */
  2011. unixNewSyscallReturn:
  2012.     lw        s2, proc_RunningProcesses    # s2 <= pointer to running
  2013.                         #       processes array.
  2014.     lw        s1, machKcallTableOffset    # s1 <= Offset of kcall table
  2015.                         #       in proc table entry.
  2016.     lw        s2, 0(s2)            # s2 <= pointer to currently
  2017.                         #       running process
  2018.     add        s1, s1, 4            # Special handling flag follows
  2019.                         # kcallTable field. 
  2020.     add        s1, s2, s1            # s1 <= pointer to special
  2021.                         #       handling flag.
  2022. /*
  2023.  * We now have the following saved information:
  2024.  *
  2025.  *    s0:    machCurStatePtr
  2026.  *    s1:    procPtr->specialHandling
  2027.  *    s2:    procPtr
  2028.  *    s3:    syscall type.
  2029.  *    s8:    status register
  2030.  */
  2031. /*
  2032.  * Set up the registers correctly:
  2033.  *
  2034.  *    1) Restore a0, a1, a2 and v1
  2035.  *    2) If status == 0 then regs[a3] <- 0 and v0 <- return value.
  2036.  *       Else regs[A3] <- 1 and v0 <- Compat_MapCode(status).
  2037.  */
  2038. 1:
  2039.     bltz    v0, 3f
  2040.     add        a3, zero, zero
  2041. 2:
  2042.     lw        a0, MACH_TRAP_REGS_OFFSET + (A0 * 4)(s0)
  2043.     lw        a1, MACH_TRAP_REGS_OFFSET + (A1 * 4)(s0)
  2044.     lw        a2, MACH_TRAP_REGS_OFFSET + (A2 * 4)(s0)
  2045.     lw        v1, MACH_TRAP_REGS_OFFSET + (V1 * 4)(s0)
  2046.     sw        v0, MACH_TRAP_REGS_OFFSET + (V0 * 4)(s0)
  2047.     sw        a3, MACH_TRAP_REGS_OFFSET + (A3 * 4)(s0)
  2048.     j        sysCallReturn
  2049.     nop
  2050. 3:
  2051.     lw          v0, proc_RunningProcesses       # v0 <= pointer to running
  2052.                                                 #       processes array.
  2053.     nop
  2054.     lw          v0, 0(v0)                       # v0 <= pointer to currently
  2055.                                                 #       running process
  2056.     li          a3, 1
  2057.     lw          v0, MACH_UNIX_ERRNO_OFFSET(v0)
  2058.     nop
  2059.     j           2b
  2060.     nop
  2061.  
  2062. .end MachSysCall
  2063. .set reorder
  2064.  
  2065.     .globl MachFetchArgs
  2066. MachFetchArgs:
  2067. /*----------------------------------------------------------------------------
  2068.  *
  2069.  * MachFetch?Args --
  2070.  *
  2071.  *    Fetch the given number of arguments from the user's stack and put
  2072.  *    them onto the kernel's stack.  The user's stack pointer is in t2.
  2073.  *
  2074.  * Results:
  2075.  *         None.
  2076.  *
  2077.  * Side effects:
  2078.  *    None.
  2079.  *
  2080.  *----------------------------------------------------------------------------
  2081.  */
  2082. .set noreorder
  2083.     .globl MachFetch0Args
  2084. MachFetch0Args:
  2085.     j        ra
  2086.     subu    sp, sp, MACH_STAND_FRAME_SIZE
  2087.  
  2088.     .globl MachFetch1Arg
  2089. MachFetch1Arg:
  2090.     lw        s0, 16(t2)    
  2091.     subu    sp, sp, MACH_STAND_FRAME_SIZE + 4
  2092.     j        ra
  2093.     sw        s0, 16(sp)
  2094.  
  2095.     .globl MachFetch2Args
  2096. MachFetch2Args:
  2097.     lw        s0, 16(t2)    
  2098.     lw        s1, 20(t2)
  2099.     subu    sp, sp, MACH_STAND_FRAME_SIZE + 8
  2100.     sw        s0, 16(sp)
  2101.     j        ra
  2102.     sw        s1, 20(sp)
  2103.  
  2104.     .globl MachFetch3Args
  2105. MachFetch3Args:
  2106.     lw        s0, 16(t2)    
  2107.     lw        s1, 20(t2)
  2108.     lw        s2, 24(t2)
  2109.     subu    sp, sp, MACH_STAND_FRAME_SIZE + 12
  2110.     sw        s0, 16(sp)
  2111.     sw        s1, 20(sp)
  2112.     j        ra
  2113.     sw        s2, 24(sp)
  2114.  
  2115.     .globl MachFetch4Args
  2116. MachFetch4Args:
  2117.     lw        s0, 16(t2)    
  2118.     lw        s1, 20(t2)
  2119.     lw        s2, 24(t2)
  2120.     lw        s3, 28(t2)
  2121.     subu    sp, sp, MACH_STAND_FRAME_SIZE + 16
  2122.     sw        s0, 16(sp)
  2123.     sw        s1, 20(sp)
  2124.     sw        s2, 24(sp)
  2125.     j        ra
  2126.     sw        s3, 28(sp)
  2127.  
  2128.     .globl MachFetch5Args
  2129. MachFetch5Args:
  2130.     lw        s0, 16(t2)    
  2131.     lw        s1, 20(t2)
  2132.     lw        s2, 24(t2)
  2133.     lw        s3, 28(t2)
  2134.     lw        s4, 32(t2)
  2135.     subu    sp, sp, MACH_STAND_FRAME_SIZE + 20
  2136.     sw        s0, 16(sp)
  2137.     sw        s1, 20(sp)
  2138.     sw        s2, 24(sp)
  2139.     sw        s3, 28(sp)
  2140.     j        ra
  2141.     sw        s4, 32(sp)
  2142.  
  2143.     .globl MachFetch6Args
  2144. MachFetch6Args:
  2145.     lw        s0, 16(t2)    
  2146.     lw        s1, 20(t2)
  2147.     lw        s2, 24(t2)
  2148.     lw        s3, 28(t2)
  2149.     lw        s4, 32(t2)
  2150.     lw        s5, 36(t2)
  2151.     subu    sp, sp, MACH_STAND_FRAME_SIZE + 24
  2152.     sw        s0, 16(sp)
  2153.     sw        s1, 20(sp)
  2154.     sw        s2, 24(sp)
  2155.     sw        s3, 28(sp)
  2156.     sw        s4, 32(sp)
  2157.     j        ra
  2158.     sw        s5, 36(sp)
  2159.  
  2160. .set reorder
  2161.  
  2162.     .globl MachFetchArgsEnd
  2163. MachFetchArgsEnd:
  2164.  
  2165. /*
  2166.  * Beginning of area where the kernel should be able to handle a bus error
  2167.  * (which includes size errors) while in kernel mode.
  2168.  */
  2169.  
  2170. /*
  2171.  *----------------------------------------------------------------------
  2172.  *
  2173.  * Mach_Probe --
  2174.  *
  2175.  *    Copy a block of memory from one virtual address to another handling
  2176.  *    bus errors that may occur. This    routine is intended to be used to 
  2177.  *    probe for memory mapped devices.
  2178.  *
  2179.  *    The memory interrupt must be enabled for this thing to work.
  2180.  *
  2181.  *    The ds5000 has a few quirks that make this all more complicated
  2182.  *    then it has to be.  First of all, accesses to unpopulated memory
  2183.  *    addresses don't seem to cause any sort of error, so you can't
  2184.  *    use Mach_Probe to figure out how much memory you have.  Second,
  2185.  *    an access to an invalid IO address causes an interrupt.
  2186.  *    Therefore you need to have the memory interrupt enabled to get
  2187.  *    this to work.  A read of an invalid IO address causes both a bus error
  2188.  *    and an interrupt.  The bus error handler will just ignore the bus
  2189.  *    error if it happened in Mach_Probe.  This may not be the best solution
  2190.  *    but it seems to work.
  2191.  *
  2192.  * NOTE: This trap handlers force this routine to return SYS_NO_ACCESS if an
  2193.  *     bus error occurs.
  2194.  *
  2195.  * Calling sequences:
  2196.  *
  2197.  * ReturnStatus
  2198.  * Mach_Probe(size, srcAddress, destAddress)
  2199.  *    int        size;     Size in bytes of the read to do. Must
  2200.  *                  1, 2, 4, or 8  
  2201.  *  Address    srcAddress;     Address to read from. 
  2202.  *  Address    destAddress;     Address to store the read value. 
  2203.  *    
  2204.  *
  2205.  * Results:
  2206.  *    SUCCESS if the copy worked, SYS_NO_ACCESS if there was a bus
  2207.  *    error or IO timeout, FAILURE if the memory interrupt is
  2208.  *    disabled
  2209.  *
  2210.  * Side effects:
  2211.  *    None.
  2212.  *----------------------------------------------------------------------
  2213.  */
  2214. .set noreorder
  2215. LEAF(Mach_Probe)
  2216.     /*
  2217.      * If memory interrupts aren't turned on then we can't do a
  2218.      * probe.
  2219.      */
  2220.     mfc0    t0, MACH_COP_0_STATUS_REG
  2221.     nop
  2222.     and    t0, t0, MACH_INT_MASK_3 | MACH_SR_INT_ENA_CUR
  2223.     beq    t0, MACH_INT_MASK_3 | MACH_SR_INT_ENA_CUR, 1f
  2224.     nop
  2225.     j    ra
  2226.     add    v0, zero, 1
  2227. 1:
  2228.     add    t0, zero, 1
  2229.     sw    t0, machInProbe
  2230.     /* a0 is the number of bytes
  2231.      * a1 is the src address
  2232.      * a2 is the dest address
  2233.      */
  2234.     bne     a0,1, Read2Bytes
  2235.     nop
  2236.     lbu     t0, 0(a1)
  2237.     nop
  2238.     sb    t0, 0(a2)
  2239.     b     Done
  2240.     nop
  2241. Read2Bytes:
  2242.     bne     a0, 2, Read4Bytes
  2243.     nop
  2244.     and    t0, a1, 0x1
  2245.     bne    t0, zero, BadRead
  2246.     nop
  2247.     and    t0, a2, 0x1
  2248.     bne    t0, zero, BadRead
  2249.     nop
  2250.     lhu    t0, 0(a1)
  2251.     nop
  2252.     sh    t0, 0(a2)
  2253.     b     Done
  2254.     nop
  2255. Read4Bytes:
  2256.     bne     a0, 4, Read8Bytes
  2257.     nop
  2258.     and    t0, a1, 0x3
  2259.     bne    t0, zero, BadRead
  2260.     nop
  2261.     and    t0, a2, 0x3
  2262.     bne    t0, zero, BadRead
  2263.     nop
  2264.     lw    t0, 0(a1)
  2265.     nop
  2266.     sw    t0, 0(a2)
  2267.     b     Done
  2268.     nop
  2269. Read8Bytes:
  2270.     bne     a0, 8, BadRead
  2271.     nop
  2272.     and    t0, a1, 0x7
  2273.     bne    t0, zero, BadRead
  2274.     nop
  2275.     and    t0, a2, 0x7
  2276.     bne    t0, zero, BadRead
  2277.     nop
  2278.     lw    t0, 0(a1)
  2279.     nop
  2280.     sw    t0, 0(a2)
  2281.     lw    t0, 4(a1)
  2282.     nop    
  2283.     sw    t0, 4(a1)
  2284.     b     Done
  2285.     nop
  2286. BadRead:
  2287.     j    ra
  2288.     add    v0, zero, 1
  2289. Done:
  2290.     sw    zero, machInProbe
  2291.     j    ra
  2292.     add    v0, zero, zero
  2293. .set reorder
  2294. END(Mach_Probe)
  2295.  
  2296.